#include <rclcpp/rclcpp.hpp> 
#include "std_msgs/msg/string.hpp"
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp> 
#include "geometry_msgs/msg/twist.hpp" 
#include <tf2/transform_datatypes.h>
#include <cmath>
#include <nav_msgs/msg/path.hpp>
#include "jsoncpp/json/json.h"
#include <mosquitto.h>
#include <iostream>
#include <functional>
#include "mqtt.hpp"
//#include "graceful_controller.hpp"
//namespace graceful_controller
class nav_control : public rclcpp::Node {
    public:
        nav_control() : Node("tf_reader"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) {
            timer_ = this->create_wall_timer(
                    std::chrono::milliseconds(100),
                    std::bind(&nav_control::timer_callback, this));
            ros_count=0;
           // nav_publisher_ = this->create_publisher<std_msgs::msg::String>("nav_cmd", 10);
            
             // 创建发布器（控制指令）
            cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>("/nav_cmd", 10);
            mqtc=new Mqtt_client("rec_topic","send_topic");
            mqtc->setCallback(std::bind(&nav_control::on_mqt_msg,this, std::placeholders::_1, std::placeholders::_2));
            mqtc->connect();
            //gfc = new GracefulController();
            r_max=100;
            goal_x=3.3;
            goal_y=-3.5;
            }
            
        private:
            void on_mqt_msg(const std::string& topic,const std::string& msg)
            {
               std::cout<<"topic="<<topic<<"  msg="<<msg<<ros_count<<std::endl; 
               ros_count++;
               mqtc->publish(msg);
               //mqtc->publish(msg);
            }
            void timer_callback() {  
                try {
                    geometry_msgs::msg::Twist cmd_vel;
                    // 查询目标坐标系变换（示例：从base_link到odom）
                    auto transform = tf_buffer_.lookupTransform(
                      "map", "base_link", tf2::TimePointZero);  
                    // 提取translation 
                    current_x = transform.transform.translation.x; 
                    current_y = transform.transform.translation.y; 
                    //double z = transform.transform.translation.z; 
 
                    // 提取rotation（四元数）
                    // double qx = transform.transform.rotation.x; 
                    // double qy = transform.transform.rotation.y; 
                    double qz = transform.transform.rotation.z; 
                    double qw = transform.transform.rotation.w; 
 
                        // 计算Z轴旋转角度 
                        current_w = 2*atan2(qz,qw);
                        if(current_w>3.1415)  current_w= -3.1415*2+current_w;
                        // RCLCPP_INFO(this->get_logger(), 
                        // "Translation: [%.2f, %.2f] Rotation: [%.2f",current_x,current_y,current_w);
                        Json::Value json_val;
                        // json_val["act"]["x"]=current_x;
                        // json_val["act"]["y"]=current_y;
                        // json_val["act"]["w"]=current_w;
                        
                        
                      //  nav_publisher_->publish(message);  
                    //   double wwg=-1.0-current_w;
                    //   if(wwg>3.1415)wwg=-3.1415*2+wwg;
                    //   if(wwg<-3.1415)wwg=3.1415*2+wwg;

                    //   gfc->approach(3.0-current_x,-2.0-current_y,0,
                    //                 cmd_x,cmd_w,false);      
                        double dx,dy;
                        dx=goal_x-current_x;
                        dy=goal_y-current_y;
                        double wwg= atan2(dy, dx);
                        //cmd_w= (fmod(wwg + M_PI, 2*M_PI) - M_PI)-current_w;
                        cmd_w = wwg- current_w;
                        double max_w=0.15;
                        if(cmd_w>max_w)cmd_w=max_w;
                        if(cmd_w<-max_w)cmd_w=-max_w;
                        cmd_x=0.2;
                        json_val["act"]["x"]=cmd_x;
                        json_val["act"]["y"]=0;
                        json_val["act"]["w"]=-cmd_w;            
                        auto message = std_msgs::msg::String();
                        message.data=json_val.toStyledString();  
                       // nav_publisher_->publish(message);    
                       double rr=sqrt((goal_x-current_x)*(goal_x-current_x)
                                        +(goal_y-current_y)*(goal_y-current_y));
                     
                       RCLCPP_INFO(this->get_logger(), 
                                        "Translation: [%.2f, %.2f] Rotation: [%.2f",
                                        current_x,current_y,current_w);  
                       RCLCPP_INFO(this->get_logger(), 
                                        "goal: [%.2f, %.2f] w=%.2f",
                                        dx,dy,wwg);                  
                       RCLCPP_INFO(this->get_logger(), "rr=%6.3f vx=%6.3f  vw=%6.3f",rr,cmd_x,cmd_w);

                        if(rr>0.2)
                         {//r_max=rr;
                        // nav_publisher_->publish(message);  
                         cmd_vel.linear.x  = cmd_x;
                         cmd_vel.linear.y = 0;
                         cmd_vel.angular.z  = -cmd_w;
                         cmd_vel_pub_->publish(cmd_vel);
                         }
                    } catch (const tf2::TransformException & ex) {
                        RCLCPP_ERROR(this->get_logger(), "TF异常: %s", ex.what()); 
                    }

                   // mqtc->publish_message();
                }
      
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr nav_publisher_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
  double current_x,current_y,current_w;
  double goal_x,goal_y,goal_w;
  double cmd_x,cmd_w;
  int ros_count;
  Mqtt_client *mqtc;
 // GracefulControllerPtr gfc;
 // GracefulController *gfc;
  double r_max;
};  
int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<nav_control>();
   
    rclcpp::spin(node);
   
    rclcpp::shutdown();
    return 0;
  }